#include "usr_inc.h"

/* 底盘状态 */
ChassisState_t chassisState = CHASSIS_FREE;

/* 底盘速度指令 */
ChassisSpdCmdMsg_t spdCmd = {0};

/* 左上舵轮 */
GM6020_t gm1;
Pid_t velPid1;
Pid_t posPid1;
Dgm_t dgmLeftUp;
float speedLeftUp = 0.0;

/* 左下舵轮 */
GM6020_t gm2;
Pid_t velPid2;
Pid_t posPid2;
Dgm_t dgmLeftDown;
float speedLeftDown = 0.0;

/* 右下舵轮 */
GM6020_t gm3;
Pid_t velPid3;
Pid_t posPid3;
Dgm_t dgmRightDown;
float speedRightDown = 0.0;

/* 右上舵轮 */
GM6020_t gm4;
Pid_t velPid4;
Pid_t posPid4;
Dgm_t dgmRightUp;
float speedRightUp = 0.0;

/**
 * @brief 初始化所有底盘相关的实例
 */
void ALL_CHASSIS_Init(void)
{
    PidInit_t pidInit = {0};

    /* 左上舵轮 */
    {
        GM6020_Init(&gm1, 0x01, GM_VOLTAGE_CONTROL, &hfdcan1);

        pidInit.kp = 170.0;
        pidInit.ki = 0.0;
        pidInit.kd = 0.0;
        pidInit.form = PID_POSITIONAL_FORM;
        pidInit.integralLimit = 20000 / 1.5;
        pidInit.maxOutput = 20000;
        pidInit.minOutput = -20000;
        PID_Init(&velPid1, &pidInit);

        pidInit.kp = 15.0;
        pidInit.ki = 0.0;
        pidInit.kd = 0.0;
        pidInit.form = PID_POSITIONAL_FORM;
        pidInit.integralLimit = 270 / 3;
        pidInit.maxOutput = 270;
        pidInit.minOutput = -270;
        PID_Init(&posPid1, &pidInit);

        DGM_Init(&dgmLeftUp, 0x02, &hfdcan1);
    }

    /* 左下舵轮 */
    {
        GM6020_Init(&gm2, 0x02, GM_VOLTAGE_CONTROL, &hfdcan1);

        pidInit.kp = 130.0;
        pidInit.ki = 0.0;
        pidInit.kd = 0.0;
        pidInit.form = PID_POSITIONAL_FORM;
        pidInit.integralLimit = 20000 / 1.5;
        pidInit.maxOutput = 20000;
        pidInit.minOutput = -20000;
        PID_Init(&velPid2, &pidInit);

        pidInit.kp = 14.5;
        pidInit.ki = 0.0;
        pidInit.kd = 0.0;
        pidInit.form = PID_POSITIONAL_FORM;
        pidInit.integralLimit = 270 / 3;
        pidInit.maxOutput = 270;
        pidInit.minOutput = -270;
        PID_Init(&posPid2, &pidInit);

        DGM_Init(&dgmLeftDown, 0x03, &hfdcan1);
    }

    /* 右下舵轮 */
    {
        GM6020_Init(&gm3, 0x03, GM_VOLTAGE_CONTROL, &hfdcan2);

        pidInit.kp = 130.0;
        pidInit.ki = 0.0;
        pidInit.kd = 0.0;
        pidInit.form = PID_POSITIONAL_FORM;
        pidInit.integralLimit = 20000 / 1.5;
        pidInit.maxOutput = 20000;
        pidInit.minOutput = -20000;
        PID_Init(&velPid3, &pidInit);

        pidInit.kp = 15.0;
        pidInit.ki = 0.0;
        pidInit.kd = 0.0;
        pidInit.form = PID_POSITIONAL_FORM;
        pidInit.integralLimit = 270 / 3;
        pidInit.maxOutput = 270;
        pidInit.minOutput = -270;
        PID_Init(&posPid3, &pidInit);

        DGM_Init(&dgmRightDown, 0x05, &hfdcan2);
    }

    /* 右上舵轮 */
    {
        GM6020_Init(&gm4, 0x04, GM_VOLTAGE_CONTROL, &hfdcan2);

        pidInit.kp = 175.0;
        pidInit.ki = 0.0;
        pidInit.kd = 0.0;
        pidInit.form = PID_POSITIONAL_FORM;
        pidInit.integralLimit = 20000 / 1.5;
        pidInit.maxOutput = 20000;
        pidInit.minOutput = -20000;
        PID_Init(&velPid4, &pidInit);

        pidInit.kp = 14.0;
        pidInit.ki = 0.0;
        pidInit.kd = 0.0;
        pidInit.form = PID_POSITIONAL_FORM;
        pidInit.integralLimit = 270 / 3;
        pidInit.maxOutput = 270;
        pidInit.minOutput = -270;
        PID_Init(&posPid4, &pidInit);

        DGM_Init(&dgmRightUp, 0x07, &hfdcan2);
    }
}

void StartChassisUpdate(void *argument)
{
    for (;;)
    {
        /* 更新速度指令 */
        {
            if (xQueueReceive(spdCmdQueueHandle, &spdCmd, 1000) != pdTRUE)
            {
                spdCmd.vX = 0;
                spdCmd.vY = 0;
                spdCmd.w = 0;
            }
        }
    }
}

void StartLeftUpExe(void *argument)
{
    vTaskDelay(1000);
    DGM_SendCmdWithoutData(&dgmLeftUp, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmLeftUp, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmLeftUp, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmLeftUp, DGM_CMD_MOTOR_ENABLE);

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        float _vX1 = spdCmd.vX - spdCmd.w * CHASSIS_CENTER_DISTANCE * M_SQRT1_2;
        float _vY1 = spdCmd.vY + spdCmd.w * CHASSIS_CENTER_DISTANCE * M_SQRT1_2;
        float posNow = GM6020_GetPos(gm1InfoBoxHandle, GM1_BASECNT);

        if (chassisState == CHASSIS_FREE)
        {
            CHASSIS_OptimizeArc(_vX1, _vY1, posNow, &posPid1, &speedLeftUp);
        }
        else if (chassisState == CHASSIS_LOCK)
        {
            CHASSIS_OptimizeArc(1.0, 1.0, posNow, &posPid1, &speedLeftUp); // 45°
            speedLeftUp = 0.0;
        }

        PID_Calculate(&posPid1, posNow);
        PID_SetTarget(&velPid1, PID_GetOutput(&posPid1));
        PID_Calculate(&velPid1, GM6020_GetRpm(gm1InfoBoxHandle));
        GM6020_WriteCmdBuf(&gm1, PID_GetOutput(&velPid1), djiMutexHandle);

        vTaskDelay(pdMS_TO_TICKS(1));
        DGM_SendCmdWithFloat(&dgmLeftUp, DGM_CMD_SET_VELOCITY, speedLeftUp * cosf((posPid1.target - posNow) / 180.0 * M_PI));

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartLeftDownExe(void *argument)
{
    vTaskDelay(1000);
    DGM_SendCmdWithoutData(&dgmLeftDown, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmLeftDown, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmLeftDown, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmLeftDown, DGM_CMD_MOTOR_ENABLE);

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        float _vX2 = spdCmd.vX - spdCmd.w * CHASSIS_CENTER_DISTANCE * M_SQRT1_2;
        float _vY2 = spdCmd.vY - spdCmd.w * CHASSIS_CENTER_DISTANCE * M_SQRT1_2;
        float posNow = GM6020_GetPos(gm2InfoBoxHandle, GM2_BASECNT);

        if (chassisState == CHASSIS_FREE)
        {
            CHASSIS_OptimizeArc(_vX2, _vY2, posNow, &posPid2, &speedLeftDown);
        }
        else if (chassisState == CHASSIS_LOCK)
        {
            CHASSIS_OptimizeArc(-1.0, 1.0, posNow, &posPid2, &speedLeftDown); // -45°
            speedLeftDown = 0.0;
        }

        PID_Calculate(&posPid2, posNow);
        PID_SetTarget(&velPid2, PID_GetOutput(&posPid2));
        PID_Calculate(&velPid2, GM6020_GetRpm(gm2InfoBoxHandle));
        GM6020_WriteCmdBuf(&gm2, PID_GetOutput(&velPid2), djiMutexHandle);

        vTaskDelay(pdMS_TO_TICKS(1));
        DGM_SendCmdWithFloat(&dgmLeftDown, DGM_CMD_SET_VELOCITY, speedLeftDown * cosf((posPid2.target - posNow) / 180.0 * M_PI));

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartRightDownExe(void *argument)
{
    vTaskDelay(1000);
    DGM_SendCmdWithoutData(&dgmRightDown, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmRightDown, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmRightDown, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmRightDown, DGM_CMD_MOTOR_ENABLE);

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        float _vX3 = spdCmd.vX + spdCmd.w * CHASSIS_CENTER_DISTANCE * M_SQRT1_2;
        float _vY3 = spdCmd.vY - spdCmd.w * CHASSIS_CENTER_DISTANCE * M_SQRT1_2;
        float posNow = GM6020_GetPos(gm3InfoBoxHandle, GM3_BASECNT);

        if (chassisState == CHASSIS_FREE)
        {
            CHASSIS_OptimizeArc(_vX3, _vY3, posNow, &posPid3, &speedRightDown);
        }
        else if (chassisState == CHASSIS_LOCK)
        {
            CHASSIS_OptimizeArc(1.0, 1.0, posNow, &posPid3, &speedRightDown); // 45°
            speedRightDown = 0.0;
        }

        PID_Calculate(&posPid3, posNow);
        PID_SetTarget(&velPid3, PID_GetOutput(&posPid3));
        PID_Calculate(&velPid3, GM6020_GetRpm(gm3InfoBoxHandle));
        GM6020_WriteCmdBuf(&gm3, PID_GetOutput(&velPid3), djiMutexHandle);

        vTaskDelay(pdMS_TO_TICKS(1));
        DGM_SendCmdWithFloat(&dgmRightDown, DGM_CMD_SET_VELOCITY, speedRightDown * cosf((posPid3.target - posNow) / 180.0 * M_PI));

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartRightUpExe(void *argument)
{
    vTaskDelay(1000);
    DGM_SendCmdWithoutData(&dgmRightUp, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmRightUp, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmRightUp, DGM_CMD_MOTOR_ENABLE);
    vTaskDelay(10);
    DGM_SendCmdWithoutData(&dgmRightUp, DGM_CMD_MOTOR_ENABLE);

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        float _vX4 = spdCmd.vX + spdCmd.w * CHASSIS_CENTER_DISTANCE * M_SQRT1_2;
        float _vY4 = spdCmd.vY + spdCmd.w * CHASSIS_CENTER_DISTANCE * M_SQRT1_2;
        float posNow = GM6020_GetPos(gm4InfoBoxHandle, GM4_BASECNT);

        if (chassisState == CHASSIS_FREE)
        {
            CHASSIS_OptimizeArc(_vX4, _vY4, posNow, &posPid4, &speedRightUp);
        }
        else if (chassisState == CHASSIS_LOCK)
        {
            CHASSIS_OptimizeArc(-1.0, 1.0, posNow, &posPid4, &speedRightUp); // -45°
            speedRightUp = 0.0;
        }

        PID_Calculate(&posPid4, posNow);
        PID_SetTarget(&velPid4, PID_GetOutput(&posPid4));
        PID_Calculate(&velPid4, GM6020_GetRpm(gm4InfoBoxHandle));
        GM6020_WriteCmdBuf(&gm4, PID_GetOutput(&velPid4), djiMutexHandle);

        vTaskDelay(pdMS_TO_TICKS(1));
        DGM_SendCmdWithFloat(&dgmRightUp, DGM_CMD_SET_VELOCITY, speedRightUp * cosf((posPid4.target - posNow) / 180.0 * M_PI));

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartDgmErrorHandle(void *argument)
{
    uint16_t leftUpCnt = 0;
    uint16_t leftDownCnt = 0;
    uint16_t rightDownCnt = 0;
    uint16_t rightUpCnt = 0;

    // vTaskSuspend(NULL);

    for (const int16_t period = 5; ;)
    {
        DgmStatuswordMsg_t msg;
        xQueueReceive(dgmStatuswordMsgQueueHandle, &msg, portMAX_DELAY);

        if (msg.id == dgmLeftUp.id)
        {
            if (msg.error != 0)
            {
                vTaskDelay(100);
                DGM_SendCmdWithoutData(&dgmLeftUp, DGM_CMD_ERROR_RESET);
                vTaskDelay(10);
                DGM_SendCmdWithoutData(&dgmLeftUp, DGM_CMD_MOTOR_ENABLE);
                leftUpCnt++;
            }
            printf("t0.txt=\" DGM1: %#x, %#x, %d\"\xff\xff\xff", msg.state, msg.error, leftUpCnt);
        }
        else if (msg.id == dgmLeftDown.id)
        {
            if (msg.error != 0)
            {
                vTaskDelay(100);
                DGM_SendCmdWithoutData(&dgmLeftDown, DGM_CMD_ERROR_RESET);
                vTaskDelay(10);
                DGM_SendCmdWithoutData(&dgmLeftDown, DGM_CMD_MOTOR_ENABLE);
                leftDownCnt++;
            }
            printf("t1.txt=\" DGM2: %#x, %#x, %d\"\xff\xff\xff", msg.state, msg.error, leftDownCnt);
        }
        else if (msg.id == dgmRightDown.id)
        {
            if (msg.error != 0)
            {
                vTaskDelay(100);
                DGM_SendCmdWithoutData(&dgmRightDown, DGM_CMD_ERROR_RESET);
                vTaskDelay(10);
                DGM_SendCmdWithoutData(&dgmRightDown, DGM_CMD_MOTOR_ENABLE);
                rightDownCnt++;
            }
            printf("t2.txt=\" DGM3: %#x, %#x, %d\"\xff\xff\xff", msg.state, msg.error, rightDownCnt);
        }
        else if (msg.id == dgmRightUp.id)
        {
            if (msg.error != 0)
            {
                vTaskDelay(100);
                DGM_SendCmdWithoutData(&dgmRightUp, DGM_CMD_ERROR_RESET);
                vTaskDelay(10);
                DGM_SendCmdWithoutData(&dgmRightUp, DGM_CMD_MOTOR_ENABLE);
                rightUpCnt++;
            }
            printf("t3.txt=\" DGM4: %#x, %#x, %d\"\xff\xff\xff", msg.state, msg.error, rightUpCnt);
        }

        vTaskDelay(pdMS_TO_TICKS(period));
    }
}

void StartChassisPrintf(void *argument)
{
    vTaskSuspend(NULL);

    for (const int16_t period = 30; ;)
    {
        PidArgsMsg_t msg;
        if (xQueueReceive(pidArgsQueueHandle, &msg, 5) == pdTRUE)
        {
            /* velPid实时调参 */
            // PID_SetArgs(&velPid1, msg.kp, msg.ki, msg.kd);
            // PID_SetTarget(&velPid1, msg.target);

            /* posPid实时调参 */
            PID_SetArgs(&posPid4, msg.kp, msg.ki, msg.kd);
            PID_SetTarget(&posPid4, msg.target);

            /* dgm调试 */
            // speedRightUp = msg.target;
        }

        // printf("%d\r\n", GM6020_GetCnt(gm1InfoBoxHandle));
        printf("%f, %f, %f\r\n", GM6020_GetPos(gm4InfoBoxHandle, GM4_BASECNT), PID_GetTarget(&posPid4), GM6020_GetRpm(gm4InfoBoxHandle));
        // printf("%f, %f\r\n", PID_GetTarget(&posPid1), speedLeftUp);

        vTaskDelay(pdMS_TO_TICKS(period));
    }
}

/**
 * @brief 处理舵轮轮系的优劣弧
 * @param _vX: 轮系的x轴速度分量
 * @param _vY: 轮系的y轴速度分量
 * @param posNow: 舵向电机的当前位置
 * @param posPid: 舵向电机的外环pid实例
 * @param spd: 轮毂电机的速度
 * @retval None
 */
void CHASSIS_OptimizeArc(float _vX, float _vY, float posNow, Pid_t *posPid, float *spd)
{
    float posTar = ALGO_Rad2Deg(-atan2f(_vY, _vX)); // 转变为编码器的正方向
    float posErr = ALGO_WrapTo180(posTar - posNow);
    if (posErr > 90.0 && posErr <= 180.0)
    {
        PID_SetTarget(posPid, posNow + posErr - 180.0);
        *spd = (-1) * hypotf(_vX, _vY) / WHEEL_R / (2 * M_PI);
    }
    else if (posErr >= -180.0 && posErr < -90.0)
    {
        PID_SetTarget(posPid, posNow + posErr + 180.0);
        *spd = (-1) * hypotf(_vX, _vY) / WHEEL_R / (2 * M_PI);
    }
    else
    {
        PID_SetTarget(posPid, posNow + posErr);
        *spd = hypotf(_vX, _vY) / WHEEL_R / (2 * M_PI); // m/s to r/s
    }
}
